// Copyright (c) 2012, Hazen Eckert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the The University of Texas at Dallas nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE UNIVERSITY OF TEXAS AT DALLAS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <ros/ros.h>
#include <auv_driver_common/auv_driver.h>
#include <auv_driver_common/auv_driver_node.h>
#include <strings.h>

auv_driver_node::auv_driver_node(ros::NodeHandle h):nh(h), private_nh("~"), updater()
{      
    state = auv_driver_node::CLOSED;
    updater.setHardwareID(getSerialNumber());
    updater.add("Basic Diagnostics", (auv_driver_node *)this, &auv_driver_node::update_basic_diagnostics);
    updater.add("Standard Diagnostics", (auv_driver_node *)this, &auv_driver_node::update_standard_diagnostics);
}

auv_driver_node::~auv_driver_node()
{

}

void auv_driver_node::spinOnce()
{
    boardStatus = getBoardStatus();
    updater.update();
    ros::spinOnce();
}

void auv_driver_node::update_basic_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
    if(boardStatus.compare("ERROR") || boardStatus.compare("Unknown Board Status") || boardStatus.compare("Communication ERROR")) {
        stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "AUV Driver Basic Diagnostics");
        stat.add("Processor Type", processorType);
        stat.add("Software Version", softwareVersion);
        stat.add("Processor Description", processorDescription);
        stat.add("Board Function", boardFunction);
        stat.add("Serial Number", serialNumber);
        stat.add("Board Description", boardDescription);
        stat.add("Board Status", boardStatus);
        stat.add("Max Payload", maxPayload);
    } else if (boardStatus.compare("Unitialized") || boardStatus.compare("Initialized") || boardStatus.compare("Busy")) {
        stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "AUV Driver Basic Diagnostics");
        stat.add("Processor Type", processorType);
        stat.add("Software Version", softwareVersion);
        stat.add("Processor Description", processorDescription);
        stat.add("Board Function", boardFunction);
        stat.add("Serial Number", serialNumber);
        stat.add("Board Description", boardDescription);
        stat.add("Board Status", boardStatus);
        stat.add("Max Payload", maxPayload);
    } else if(boardStatus.compare("Idle")) {
        stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "AUV Driver Basic Diagnostics");
        stat.add("Processor Type", processorType);
        stat.add("Software Version", softwareVersion);
        stat.add("Processor Description", processorDescription);
        stat.add("Board Function", boardFunction);
        stat.add("Serial Number", serialNumber);
        stat.add("Board Description", boardDescription);
        stat.add("Board Status", boardStatus);
        stat.add("Max Payload", maxPayload);
    } else {
        ROS_ERROR("Unidentified board state");
    }
}

void auv_driver_node::update_standard_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
    stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "AUV Driver Standard Diagnostics");
    stat.add("Board Voltage", boardVoltage);
    stat.add("Board Current", boardCurrent);
}

void auv_driver_node::getDriverInfo()
{
		if (state != auv_driver_node::CLOSED) {
	    processorType = getProcessorType();
	    softwareVersion = getSoftwareVersion();
	    processorDescription = getProcessorDescription();
	    boardFunction = getBoardFunction();
	    serialNumber = getSerialNumber();
	    boardDescription = getBoardDescription();
	    boardStatus = getBoardStatus();
	    maxPayload = getMaxPayload();
        boardVoltage = getBoardVoltage();
        boardCurrent = getBoardCurrent();
    }
}

std::string auv_driver_node::getProcessorType()
{
    uint8_t family, specific;
    if (state == auv_driver_node::CLOSED) {
        return "Not Connected";
    }
    
    if (driver->getProcessorType(&family, &specific) != 0) {
        ROS_WARN("Could not get processor type from driver");
        return "Communication ERROR";
    }
    
    switch (family) {
        case 0x10:
            if (specific == 0x01) {
                return "MSP430F2013";
            } else if (specific == 0x02) {
                return "MSP430F5419";
            } else {
                return "Uknown MSP430";
            }
            break;
        
        case 0x11:
            if (specific == 0x01) {
                return "TMS320F28027";
            } else if (specific == 0x02) {
                return "TMS320F28335";
            } else {
                return "Uknown TMS320";
            }
            break;
            
        case 0x20:
            if (specific == 0x00) {
                return "AT90CAN128";
            } else if (specific == 0x01) {
                return "ATmega256x";
            } else if (specific == 0x02) {
                return "ATmega644";
            } else {
                return "Uknown AVR";
            }
            break;
            
        case 0x21:
            if (specific == 0x00) {
                return "EP3C25Q240C8N";
            } else {
                return "Uknown Altera FPGA";
            }
            break;
            
        default:
            return "Uknown Processor Family";
            break;
    }
}

std::string auv_driver_node::getSoftwareVersion()
{
    if (state == auv_driver_node::CLOSED) {
        return "Not Connected";
    }
    
    uint16_t major, minor, incremental;
    
    if (driver->getSoftwareVersion(&major, &minor, &incremental) != 0) {
        ROS_WARN("Could not get software type from driver");
        return "Communication ERROR";
    }
    
    char buffer[255];
    sprintf(buffer, "%d.%d.%d", (int)major, (int)minor, (int)incremental);
    std::string softwareVersion_ (buffer);
    return softwareVersion_;
}

std::string auv_driver_node::getProcessorDescription()
{
    if (state == auv_driver_node::CLOSED) {
        return "Not Connected";
    }
    
    char *buffer = NULL;
    
    if (driver->getProcessorDescription(&buffer) != 0) {
        ROS_WARN("Could not get processor description from driver");
        return "Communication ERROR";
    }
    
    std::string processorDescription_ (buffer);
    free(buffer);
    return processorDescription_;
}

std::string auv_driver_node::getBoardFunction()
{
    if (state == auv_driver_node::CLOSED) {
        return "Not Connected";
    }
    
    uint8_t boardFunction;
    
    if (driver->getBoardFunction(&boardFunction) != 0) {
        ROS_WARN("Could not get board function from driver");
        return "Communication ERROR";
    }
    
    switch (boardFunction) {
        case 0x00:
            return "Unspecified";
            break;
            
        case 0x01:
            return "Depth Sensor";
            break;
            
        case 0x02:
            return "Motor Controller";
            
        case 0x03:
            return "Passive DF Sonar";
            
        case 0x04:
            return "Mixed Set #1";
            
        default:
            return "Unknown Board Type";
            break;
    }
}

std::string auv_driver_node::getSerialNumber()
{
    if (state == auv_driver_node::CLOSED) {
        return "Not Connected";
    }
    
    char *buffer = NULL;
    
    if (driver->getBoardSerialNumber(&buffer) != 0) {
        ROS_WARN("Could not get serial number from driver");
        return "Communication ERROR";
    }
    
    std::string serialNumber_ (buffer);
    free(buffer);
    return serialNumber_;
}

std::string auv_driver_node::getBoardDescription()
{
    if (state == auv_driver_node::CLOSED) {
        return "Not Connected";
    }
    
    char *buffer = NULL;
    
    if (driver->getBoardDescription(&buffer) != 0) {
        ROS_WARN("Could not get board description from driver");
        return "Communication ERROR";
    }
    
    std::string boardDescription_ (buffer);
    free(buffer);
    return boardDescription_;
}

std::string auv_driver_node::getBoardStatus()
{
    if (state == auv_driver_node::CLOSED) {
        return "Not Connected";
    }
    
    uint8_t boardStatus;
    
    if (driver->getBoardStatus(&boardStatus) != 0) {
        ROS_WARN("Could not get board status from driver");
        return "Communication ERROR";
    }
    
    switch (boardStatus) {
        case 0x00:
            return "ERROR";
            break;
            
        case 0x01:
            return "Unitialized";
            break;
            
        case 0x02:
            return "Initialized";
            
        case 0x03:
            return "Idle";
            
        case 0x04:
            return "Busy";
            
        default:
            return "Unknown Board Status";
            break;
    }
}

int auv_driver_node::getMaxPayload()
{
    if (state == auv_driver_node::CLOSED) {
        return -1;
    }
    
    uint8_t requestBufferSize, responseBufferSize;
    
    if (driver->getMaxPayload(&requestBufferSize, &responseBufferSize) != 0) {
        ROS_WARN("Could not get max payload from driver");
        return 0;
    }
    
    return requestBufferSize;
}

float auv_driver_node::getBoardVoltage()
{
    if (state == auv_driver_node::CLOSED) {
        return -1;
    }
    
    float voltage;
    
    if (driver->getBoardVoltage(&voltage) != 0) {
        ROS_WARN("Could not get board voltage from driver");
        return 0;
    }
    
    return voltage;
}

float auv_driver_node::getBoardCurrent()
{
    if (state == auv_driver_node::CLOSED) {
        return -1;
    }
    
    float current;
    
    if (driver->getBoardCurrent(&current) != 0) {
        ROS_WARN("Could not get board current from driver");
        return 0;
    }
    
    return current;
}

